#include "epuckDriver/configFile.hpp"

void ConfigFile::readConfigEpucks() {

    TiXmlElement *epuckCfg = this->xmlFile->FirstChildElement("Epuck");

    for (; epuckCfg; epuckCfg = epuckCfg->NextSiblingElement("Epuck")) {
        InfoEpuck epuck = readInfoEpuck(epuckCfg);
        infoNode.push_back(epuck);
    }
}

InfoEpuck ConfigFile::readInfoEpuck(TiXmlElement *info) {

    InfoEpuck infoEpuck;
    const char *val = "value\0";

    infoEpuck.node = info->FirstChildElement("node")->Attribute(val);
     
 //   infoEpuck.id = info->FirstChildElement("id")->Attribute(val);

    std::string strInfo;

    strInfo = info->FirstChildElement("cmdvelocity")->Attribute(val);
    if (!strInfo.compare("true"))
        infoEpuck.cmdVelocity = true;
    else
        infoEpuck.cmdVelocity = false;

    strInfo = info->FirstChildElement("cmdcamera")->Attribute(val);
    if (!strInfo.compare("true"))
        infoEpuck.cmdCamera = true;
    else
        infoEpuck.cmdCamera = false;

    strInfo = info->FirstChildElement("ledfront")->Attribute(val);
    if (!strInfo.compare("true"))
        infoEpuck.cmdLEDFront = true;
    else
            infoEpuck.cmdLEDFront = false;
        
        strInfo = info->FirstChildElement("ledbody")->Attribute(val);
        if (!strInfo.compare("true"))
            infoEpuck.cmdLEDBody = true;
        else
            infoEpuck.cmdLEDBody = false;
        
        strInfo = info->FirstChildElement("ledring")->Attribute(val);
        if (!strInfo.compare("true"))
            infoEpuck.cmdLEDRing = true;
        else
            infoEpuck.cmdLEDRing = false;

        strInfo = info->FirstChildElement("cmdir")->Attribute(val);
        if (!strInfo.compare("true"))
            infoEpuck.cmdIR = true;
        else
            infoEpuck.cmdIR = false;
        
        strInfo = info->FirstChildElement("serviceServer")->Attribute(val);
        if (!strInfo.compare("true"))
            infoEpuck.serviceServer = true;
        else
            infoEpuck.serviceServer = false;

    infoEpuck.port = info->FirstChildElement("port")->Attribute(val);
    

    return infoEpuck;
}
